#include <iostream>
#include <fstream>
#include <string>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <vector>

#include "cv.h"
#include "highgui.h"

#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/visualization/cloud_viewer.h>

#include <Eigen/Core> 
#include <Eigen/Geometry>

#include "graph/VertexCamera.h"
#include "graph/VertexPlane.h"
#include "graph/VertexPoint3D.h"
#include "graph/EdgeCameraPlane.h"



#include "FeatureDescriptor.h"
#include "SurfFeatureDescriptor64.h"
#include "SurfFeatureDescriptor128.h"
#include "OrbFeatureDescriptor.h"
#include "FloatHistogramFeatureDescriptor.h"

#include "Point.h"
#include "Plane.h"
#include "Line.h"
#include "KeyPoint.h"
#include "KeyPointSet.h"

#include "Association.h"
#include "KeyPointAssociation.h"
#include "PlaneAssociation.h"
#include "AssociationSet.h"

#include "FeatureExtractor.h"
#include "SurfExtractor.h"
#include "OrbExtractor.h"

#include "Map3D.cpp"
#include "RGBDFrame.cpp"
#include "Transformation.cpp"

#include "FeatureExtractor.cpp"
#include "SurfExtractor.cpp"
#include "OrbExtractor.cpp"
#include "FPFHExtractor.cpp"
#include "OwnFeatureExtractor.cpp"


#include "FrameMatcher.cpp"
#include "DummyMatcher.cpp"
#include "SurfRansacMatcherI.cpp"
#include "JCMergeMatcherI.cpp"
#include "JCSearchMatcherI.cpp"
#include "BasicIcpMatcher.cpp"
#include "BasicGIcpMatcher.cpp"
#include "NDTMatcher.cpp"
#include "KeyPointIcpMatcherI.cpp"
#include "AICK.cpp"
#include "dnet.cpp"
#include "PlanePostProcessMatcher.cpp"
#include "SACIAMatcher.cpp"
#include "RansacPCLCopyMatcher.cpp"
#include "RansacPCLCopySlowMatcher.cpp"

#include <pthread.h>
#include <sys/types.h>
#include <dirent.h>
#include <errno.h>

using namespace std;
string out_path = "output2/";
FrameMatcher * matcher;
Transformation ** transformations;


pthread_mutex_t tasks_mutex = PTHREAD_MUTEX_INITIALIZER;
vector< int > * tasks;
vector< RGBDFrame * > * frames;

pthread_mutex_t done_tasks_mutex = PTHREAD_MUTEX_INITIALIZER;
int done_tasks = 0;

void * start_test_thread( void *ptr )
{
	while(true)
	{
		pthread_mutex_lock( &tasks_mutex );
		if(tasks->size() > 0)
		{
			int t = tasks->back();
			tasks->pop_back();
			pthread_mutex_unlock(&tasks_mutex);
			//printf("doing task %i\n",t);
			RGBDFrame * src = frames->at(t-1);
			RGBDFrame * dst = frames->at(t);
			
			transformations[t-1] = matcher->getTransformation(src,dst);
			
			pthread_mutex_lock(&done_tasks_mutex);
			done_tasks++;
			pthread_mutex_unlock(&done_tasks_mutex);
		}else{
			pthread_mutex_unlock(&tasks_mutex);
			usleep(1000);
		}
	}
}

int nr_done_tasks(){
    pthread_mutex_lock(&done_tasks_mutex);
	int val = done_tasks;
	pthread_mutex_unlock(&done_tasks_mutex);
	return val;
}

vector< RGBDFrame * > * getFrameData(string path,Map3D * map, int max){
	vector< RGBDFrame * > * all_frames = new vector< RGBDFrame * >();
	transformations = new Transformation * [max];
	for(int i = 1; i <= max; i+=1){
		transformations[i-1] = 0;
		Frame_input * fi =  new Frame_input();
		char fpath [150];
		sprintf(fpath,"%s/rgb_frames/%05i.png",path.c_str(),2600+3*i);
		printf("path:%s\n",fpath);
		fi->rgb_path 			= string(fpath);
		sprintf(fpath,"%s/depth_frames/%05i.png",path.c_str(),2600+3*i);
		printf("path:%s\n",fpath);
		fi->depth_path			= string(fpath);
		fi->calibration			= map->extractor->calibration;
		RGBDFrame * frame = new RGBDFrame(fi,map);
		if(frame->keypoints->valid_key_points.size() >= 100){
			all_frames->push_back(frame);
		}else{printf("skip...\n");}
	}
	return all_frames;
}

using namespace Eigen;

bool goToNext = false;
void keyboardEventOccurred (const pcl::visualization::KeyboardEvent &event,
                            void* viewer_void)
{
	printf("key pressed\n");
	goToNext = true;

}
//void test(boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer2,vector< RGBDFrame * > * all_frames)
void test(vector< RGBDFrame * > * all_frames)
{

	
	printf("starting test\n");
	tasks = new vector<int>();
	for(int i = 1; i < (int)all_frames->size(); i++){
		tasks->push_back(i);
	}
    
	pthread_mutex_lock( &tasks_mutex );
	for(int i = 0; i < 10; i++){
		pthread_t mythread;
		pthread_create( &mythread, NULL, start_test_thread, NULL);
	}
    pthread_mutex_unlock(&tasks_mutex);
    
	struct timeval test_start, test_end;
	gettimeofday(&test_start, NULL);
	int added_tasks = all_frames->size() -1;
	while(nr_done_tasks() < added_tasks){
		gettimeofday(&test_end, NULL);
		float test_time = (test_end.tv_sec*1000000+test_end.tv_usec-(test_start.tv_sec*1000000+test_start.tv_usec))/1000000.0f;
		printf("%i/%i Time spent: %f\n",nr_done_tasks(),added_tasks,test_time);
		usleep(500000);
	}
	

	int total_points = 0;
	for(int i = 0; i < all_frames->size(); i++){
		total_points+=all_frames->at(i)->xyz_->width*all_frames->at(i)->xyz_->height;
		//printf("total_points:%i\n",total_points);
	}
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
	cloud->width    = total_points;
	cloud->height   = 1;
	cloud->points.resize (cloud->width * cloud->height);
	int counter = 0;
	pcl::PointCloud<pcl::PointXYZ>::Ptr tmp_cloud (new pcl::PointCloud<pcl::PointXYZ>);
	
	Eigen::Matrix4f transformationMat = Eigen::Matrix4f::Identity();
	Eigen::Matrix4f current = transformationMat;
	Eigen::Matrix4f prev = transformationMat;
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
	viewer->setBackgroundColor (0.5f, 0.5f, 0.5f);
	viewer->addCoordinateSystem (1.0);
	viewer->initCameraParameters ();
	viewer->setBackgroundColor (0, 0, 0);
	viewer->registerKeyboardCallback (keyboardEventOccurred, (void*)&viewer);
	for(int i = 0; i < nr_done_tasks(); i++){
		//printf("--------------------\n");
		//printf("%i\n",i);
		//cout << transformationMat << endl;
		pcl::transformPointCloud (*(all_frames->at(i)->xyz_), *tmp_cloud, transformationMat);
		int randr = rand()%256;
		int randg = rand()%256;
		int randb = rand()%256;
		for(int j = 0; j < tmp_cloud->width*tmp_cloud->height; j++)
		{
			cloud->points[counter+j].x = tmp_cloud->points[j].x;
			cloud->points[counter+j].y = tmp_cloud->points[j].y;
			cloud->points[counter+j].z = tmp_cloud->points[j].z;
			cloud->points[counter+j].r = randr;
			cloud->points[counter+j].g = randg;
			cloud->points[counter+j].b = randb;

		}
		counter+=tmp_cloud->width*tmp_cloud->height;
		cloud->width    = counter;

		pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud);
		viewer->removePointCloud("sample cloud");
		viewer->addPointCloud<pcl::PointXYZRGB> (cloud, rgb, "sample cloud");
		viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
		//viewer->addCoordinateSystem (1.0);
		//viewer->initCameraParameters ();
		usleep(10000);
		goToNext = false;
		while (!goToNext)//!viewer->wasStopped ())
		{
			viewer->spinOnce (100);
			boost::this_thread::sleep (boost::posix_time::microseconds (100000));
		}
		
		current = (transformations[i]->transformationMatrix).inverse();
		Eigen::Matrix4f current_diff = current-prev;
		float diff = 0;// = transformation->transformationMatrix(0,0)*transformation->transformationMatrix(0,0)
		for(int i = 0; i < 3; i++)
		{
			for(int j = 0; j < 3; j++)
			{
				float d = current_diff(i,j);
				diff += d*d;
			}
		}
		float poss_diff = sqrt(current_diff(0,3)*current_diff(0,3)+current_diff(1,3)*current_diff(1,3)+current_diff(2,3)*current_diff(2,3));
		printf("weight: %f diff: %f poss_diff: %f\n",transformations[i]->weight,diff,poss_diff);
		if(diff > 0.001*transformations[i]->weight || poss_diff > 0.01*transformations[i]->weight){current = prev;}
	//if(diff > 0.1f || poss_diff > 0.1f || transformation->weight < 15 ){printf("------>weight: %f diff: %f poss_diff: %f\n",transformation->weight,diff,poss_diff);transformation->transformationMatrix = Eigen::Matrix4f::Identity();}
		transformationMat*=current;//(transformations[i]->transformationMatrix).inverse();
		prev = current;
	}
	

}



int main(int argc, char **argv)
{
	//boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
	//viewer->setBackgroundColor (0.5f, 0.5f, 0.5f);
	//viewer->addCoordinateSystem (1.0);
	//viewer->initCameraParameters ();
	
	Calibration * calib0 = new Calibration();
	calib0->fx			= 525.0;
	calib0->fy			= 525.0;
	calib0->cx			= 319.5;
	calib0->cy			= 239.5;
	calib0->ds			= 3*1;
	calib0->scale		= 5000;
	
	OrbExtractor * orb = new OrbExtractor();
	orb->nr_features = 1000;
	SurfExtractor * surf = new SurfExtractor();
	OwnFeatureExtractor * own = new OwnFeatureExtractor();
	orb->calibration = calib0;
	surf->calibration = calib0;
	own->calibration = calib0;
	
	segmenter = new RGBDSegmentation(calib0);
	
	Map3D * map = new Map3D();
	map->extractor = orb;
	vector< RGBDFrame * > * all_frames = getFrameData("/home/johane/johan_cvap_run",map,500);
	frames = all_frames;
	AICK * aick = new AICK();
	//aick->distance_threshold = 0.01;
	matcher = aick;
	//test(viewer,all_frames);
	test(all_frames);
	return 0;
}
